
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <netpacket/packet.h>
#include <net/ethernet.h>
#include <netinet/in.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <assert.h>

#include "apr_network_io.h"
#include "apr_errno.h"
#include "apr_general.h"
#include "apr_poll.h"
#include "apr_portable.h"
#include "apr_atomic.h"

#include "cbfxs.h"

#define SAMPLES_PER_CHUNK 8
#define NUM_CHANNELS 16

#define CBFXS_DEVICE "cbfxs"

#define DEF_POLL_TIMEOUT	(APR_USEC_PER_SEC * 30)

typedef struct _serv_ctx_t serv_ctx_t;

/*
 * network server context 
 */
struct _serv_ctx_t {
   apr_pool_t *mp;
   apr_socket_t *s, *cbfxs;
   apr_uint16_t rx_count, tx_count;
   apr_byte_t rx_sign[NUM_CHANNELS], tx_sign[NUM_CHANNELS];
   apr_byte_t rx_flags, tx_flags;
   apr_uint32_t spi_running;
};

static unsigned char local_eth[6] = {0}, remote_eth[6] = {0};

static void recv_net_req_cb(void *client_data)
{
   serv_ctx_t *serv_ctx = (serv_ctx_t *)client_data;

   while (1) {
      unsigned char buf[2048], *pbuf;
      unsigned char buftx[NUM_CHANNELS*SAMPLES_PER_CHUNK];
      int i;
      int sampleschunk, flags, numchannels, bytessignal;
      apr_size_t len = sizeof(buf) - 1;/* -1 for a null-terminated */

      apr_status_t rv = apr_socket_recv(serv_ctx->s, (char*)buf, &len);
      if (rv == APR_EOF || len == 0) {
	 break;
      }
      if(len <= 22) continue; /* ethernet plus TDMoE header, at least */
      pbuf = buf+14;
      if((pbuf[0] != 0) || (pbuf[1] != 0)) continue; /* subaddr == 0 */
      memcpy(local_eth, buf+6, sizeof(local_eth));
      memcpy(remote_eth, buf, sizeof(remote_eth));
      pbuf+=2;
      sampleschunk = *pbuf++; flags = *pbuf++;
      serv_ctx->rx_count = (pbuf[0] << 8) | pbuf[1]; pbuf+=2;
      numchannels = (pbuf[0] << 8) | pbuf[1]; pbuf+=2;
      bytessignal = ((numchannels+3)/4)*2;
      if(sampleschunk != SAMPLES_PER_CHUNK) continue;
      if(len < 22+bytessignal+sampleschunk*numchannels) continue; /* check sz */
      if(flags != serv_ctx->rx_flags) {
	 serv_ctx->rx_flags = flags;
	 printf("Received new TDMoE link status %d\n", serv_ctx->rx_flags);
      }
      for(i = 0; i < bytessignal; i++) {
	 apr_byte_t byt, chan;
	 byt = *pbuf++;
	 chan = (i ^ 1) * 2;
	 if(chan < NUM_CHANNELS) serv_ctx->rx_sign[chan] = byt & 0xF;
	 chan++;
	 if(chan < NUM_CHANNELS) serv_ctx->rx_sign[chan] = (byt >> 4) & 0xF;
      }
      for(i = 0; i < numchannels; i++) {
	 int j;
	 for(j = 0; j < sampleschunk; j++) {
	    buftx[j*NUM_CHANNELS+i] = *pbuf++;
	 }
      }
#if 0
      for(i = 2; i < 3; i++) {
	 int j;
	 for(j = 0; j < sampleschunk; j++) {
	    buftx[j*NUM_CHANNELS+i] = 0x7f;
	 }
      }
#endif
      len = NUM_CHANNELS*SAMPLES_PER_CHUNK;
      apr_socket_send(serv_ctx->cbfxs, (void*)buftx, &len);
   }
}

static void recv_fxs_req_cb(void *client_data)
{
   serv_ctx_t *serv_ctx = (serv_ctx_t *)client_data;

   while (1) {
      unsigned char buf[2048], *pbuf;
      unsigned char buftx[22+NUM_CHANNELS/2+4+NUM_CHANNELS*SAMPLES_PER_CHUNK];
      int i;
      int bytessignal;
      apr_size_t len = sizeof(buf) - 1;/* -1 for a null-terminated */

      apr_status_t rv = apr_socket_recv(serv_ctx->cbfxs, (char*)buf, &len);
      if (rv == APR_EOF || len == 0) {
	 break;
      }
      if(len != NUM_CHANNELS*SAMPLES_PER_CHUNK) continue;
      pbuf = buftx;
      memcpy(pbuf, local_eth, sizeof(local_eth)); pbuf += 6;
      memcpy(pbuf, remote_eth, sizeof(remote_eth)); pbuf += 6;
      *pbuf++ = 0xD0; *pbuf++ = 0x0D; /* ethernet protocol */
      *pbuf++ = 0; *pbuf++ = 0; /* TDMoE subaddress */
      *pbuf++ = SAMPLES_PER_CHUNK; *pbuf++ = serv_ctx->tx_flags;
      *pbuf++ = (serv_ctx->tx_count >> 8) & 0xFF; 
      *pbuf++ = serv_ctx->tx_count & 0xFF;
      serv_ctx->tx_count++;
      *pbuf++ = 0; *pbuf++ = NUM_CHANNELS;
      bytessignal = ((NUM_CHANNELS+3)/4)*2;
      for(i = 0; i < bytessignal; i++) {
	 apr_byte_t byt = 0, chan;
	 chan = (i ^ 1) * 2;
	 if(chan < NUM_CHANNELS) byt = serv_ctx->tx_sign[chan];
	 chan++;
	 if(chan < NUM_CHANNELS) byt |= (serv_ctx->tx_sign[chan] & 0xF) << 4;
	 *pbuf++ = byt;
      }
#if 0
      for(i = 0; i < SAMPLES_PER_CHUNK; i++) {
	 int j;
	 for(j = 0; j < NUM_CHANNELS; j++)
	    printf("%02X", buf[i*NUM_CHANNELS+j]);
	 printf(" ");
      }
      printf("\n");
#endif
      for(i = 0; i < NUM_CHANNELS; i++) {
	 int j;
	 for(j = 0; j < SAMPLES_PER_CHUNK; j++) {
	    *pbuf++ = buf[j*NUM_CHANNELS+i];
	 }
      }
      len = pbuf - buftx;
      apr_socket_send(serv_ctx->s, (void*)buftx, &len);
   }
}

struct callback_for_socket {
   void (*callback)(void *);
   void *client_data;
};

static int set_callback_for_socket(apr_socket_t *s, apr_pollset_t *pollset,
      void (*callback)(void*), void *client_data, apr_pool_t *context)
{
   struct callback_for_socket *data_callback = apr_palloc(context, 
	 sizeof(struct callback_for_socket));
   apr_pollfd_t pfd = { context, APR_POLL_SOCKET, APR_POLLIN, 0, { NULL }, 
      data_callback };
   pfd.desc.s = s;
   data_callback->callback = callback;
   data_callback->client_data = client_data;
   apr_pollset_add(pollset, &pfd);
   return 0;
}

static inline void send_legerity_command(int fd, int sz, ...)
{
   struct cbfxs_ioctl_spi_command cmd;
   va_list ap;
   int i;

   va_start(ap, sz);
   if(sz > sizeof(cmd.buffer))
      sz = sizeof(cmd.buffer);
   for(i = 0; i < sz; i++)
      cmd.buffer[i] = (unsigned char)va_arg(ap, int);
   cmd.buffer_sz = sz;
   va_end(ap);
   if(ioctl(fd, CBFXS_IOC_SPI_COMMAND, &cmd)) {
      perror("ioctl");
   }
}

static void send_legerity_configuration(int fd)
{
   // reset hardware
   send_legerity_command(fd, 1, 0x04);
   apr_sleep(apr_time_from_sec(1)*50/1000);

   // Global
   // Clock slot
   //send_legerity_command(fd, 2, 0x44, 0x09);
   send_legerity_command(fd, 2, 0x44, 0x00);
   // 8MHz
   send_legerity_command(fd, 2, 0x46, 0x0A);
   // Regulator Parameter
   send_legerity_command(fd, 4, 0xE4, 0x00, 0x04, 0x12);
   // Regulator Control
   send_legerity_command(fd, 2, 0xE6, 0x01);
   // Per Chanel
   // Enable Chanel 1 to receive Command
   send_legerity_command(fd, 2, 0x4A, 0x01);
   // TS TX 0
   send_legerity_command(fd, 2, 0x40, 0x00);
   // TS RX 0
   send_legerity_command(fd, 2, 0x42, 0x00);
   // uLaw
   //send_legerity_command(fd, 2, 0x60, 0x0f);
   send_legerity_command(fd, 2, 0x60, 0x40); // ulaw
#if 0
   send_legerity_command(fd, 2, 0x60, 0x00); // alaw
#endif
   send_legerity_command(fd, 2, 0x68, 0x16);
   send_legerity_command(fd, 3, 0x6C, 0xfe, 0xff);

   // DISN
   send_legerity_command(fd, 2, 0xCA, 0x0A);
   // ZFIR
   send_legerity_command(fd, 11, 0x98,0xB2,0xDA,0xAA,0xAC,0x5A,0xA3,
	 0xC2,0xB3,0xB3,0xAB);
   // Ziir
   send_legerity_command(fd, 6, 0x9A, 0xBC, 0xBA, 0xF7, 0x9F, 0x01);
   // RF
   send_legerity_command(fd, 15, 0x8A,0x2D,0x01,0x42,0xA0,0x33,0xA3,
	 0xD4,0x3E,0xA4,0x97,0x5B,0xAF,0x4D,0x47);
   // XF
   send_legerity_command(fd, 13, 0x88,0xAA,0x50,0xAB,0x2E,0xD2,0x46,
	 0xBC,0xC7,0x7A,0x2F,0xCA,0xB7);
   // GR
   send_legerity_command(fd, 3, 0x82, 0xA8, 0x71);
   // GX
   send_legerity_command(fd, 3, 0x80, 0xA9, 0xF0);
   // '(AX)*        0 (0.00 dB)                                          ;AX'
   // '(AR)*        0 (0.00 dB)                                          ;AR'
   // '(DRL)*       0 (0.00 dB)                                          ;DRL'
   // ANLG
   send_legerity_command(fd, 2, 0x50, 0x00);
   // BFIR   
   send_legerity_command(fd, 15, 0x86,0xA2,0x53,0xA1,0x32,0x13,0x52,
	 0xDA,0x64,0x27,0x54,0xE2,0xDE,0x2B,0xF0);
   // BIIR
   send_legerity_command(fd, 3, 0x96, 0x2E, 0x01);

   // Desligar rele de curto FXS com FXO
   send_legerity_command(fd, 2, 0x54, 0x02);
   send_legerity_command(fd, 2, 0x52, 0x01);

   // ativar
   send_legerity_command(fd, 2, 0x56, 0x23);
}

static int read_legerity_line_status(int fd)
{
   struct cbfxs_ioctl_spi_command cmd = {{0x4f, 0, 0}, 3};

   if(ioctl(fd, CBFXS_IOC_SPI_COMMAND, &cmd)) {
      perror("ioctl");
   }
   return cmd.buffer[1] & 1;
}

static int read_legerity_version(int fd)
{
   struct cbfxs_ioctl_spi_command cmd = {{0x73, 0, 0}, 3};

   if(ioctl(fd, CBFXS_IOC_SPI_COMMAND, &cmd)) {
      perror("ioctl");
   }
   return (cmd.buffer[1]<<8) | cmd.buffer[2];
}

static void* APR_THREAD_FUNC spi_thread(apr_thread_t *thd, void *data)
{
   serv_ctx_t *serv_ctx = (serv_ctx_t *)data;
   //apr_pool_t *context = apr_thread_pool_get(thd);
   int cbfxs_fd;

   apr_os_sock_get(&cbfxs_fd, serv_ctx->cbfxs);
           
   send_legerity_configuration(cbfxs_fd);

   while(apr_atomic_read32(&serv_ctx->spi_running)) {
      int linestatus;
      apr_sleep(apr_time_from_sec(1)/5);
      linestatus = read_legerity_line_status(cbfxs_fd) ? 0xD : 0x5;
      if(serv_ctx->tx_sign[0] != linestatus) {
	 serv_ctx->tx_sign[0] = linestatus;
      }
      if(serv_ctx->rx_sign[0] & 0x4) {
	 send_legerity_command(cbfxs_fd, 2, 0x56, 0x23); // active
      } else {
	 send_legerity_command(cbfxs_fd, 2, 0x56, 0x27); // ringing
      }
   }

   printf("spi_thread ending\n");
   apr_thread_exit(thd, APR_SUCCESS);
   return NULL;
}

static void closeapr(void)
{
   apr_terminate();
}

int main(int argc, const char * const argv[])
{
   int rv, i;
   apr_pool_t *context;
   apr_pollset_t *pollset;
   const apr_pollfd_t *ret_pfd = NULL;

   if (apr_app_initialize(&argc, &argv, NULL) != APR_SUCCESS) {
      fprintf(stderr, "apr_initialize problem!\n");
      exit(-1);
   }
   atexit(closeapr);

   if (apr_pool_create(&context, NULL) != APR_SUCCESS) {
      fprintf(stderr, "Something went wrong\n");
      exit(-1);
   }

   apr_pollset_create(&pollset, 100, context, 0);

   serv_ctx_t *serv_ctx = apr_palloc(context, sizeof(serv_ctx_t));
   serv_ctx->mp = context;
   serv_ctx->rx_count = serv_ctx->tx_count = 0;
   for(i = 0; i < NUM_CHANNELS; i++) serv_ctx->tx_sign[i] = 0x05;
   serv_ctx->tx_flags = 2;
   serv_ctx->rx_flags = 255;

   rv = apr_socket_create(&serv_ctx->s, AF_PACKET, SOCK_RAW, htons(0xD00D), context);
   if (rv != APR_SUCCESS) {
      fprintf(stderr, "apr_socket_create error\n");
      exit(-1);
   }

   apr_socket_opt_set(serv_ctx->s, APR_SO_NONBLOCK, 1);
   apr_socket_timeout_set(serv_ctx->s, 0);
   set_callback_for_socket(serv_ctx->s, pollset, recv_net_req_cb, serv_ctx, 
	 context);
   int s_fd;
   apr_os_sock_get(&s_fd, serv_ctx->s);

   // Determine device id
   struct ifreq ifr;
   strncpy(ifr.ifr_name, "eth0", sizeof(ifr.ifr_name));
   if (ioctl(s_fd, SIOCGIFINDEX, &ifr) == -1) {
      printf("ioctl SIOCGIFINDEX: %s\n", strerror(errno));
      exit(-1);
   }

   // Bind socket to interface
   struct sockaddr_ll sll;
   sll.sll_family      = AF_PACKET;
   sll.sll_ifindex     = ifr.ifr_ifindex;
   sll.sll_protocol    = htons(0xD00D);
   if (bind(s_fd, (struct sockaddr *) &sll, sizeof(sll)) == -1) {
      printf("bind: %s\n", strerror(errno));
      exit(-1);
   }

   int cbfxs_fd = open(CBFXS_DEVICE, O_RDWR | O_NONBLOCK);
   if(cbfxs_fd < 0) {
      fprintf(stderr, "cbfxs device open error\n");
      exit(-1);
   }
   apr_os_sock_info_t sockinfo = {&cbfxs_fd, NULL, NULL, APR_INET, SOCK_STREAM, 0};
   apr_os_sock_make(&serv_ctx->cbfxs, &sockinfo, context);
   set_callback_for_socket(serv_ctx->cbfxs, pollset, recv_fxs_req_cb, serv_ctx,
	 context);

   printf("Legerity version: %04X\n", read_legerity_version(cbfxs_fd));

   serv_ctx->spi_running = TRUE;
   apr_thread_t *spi_thd;
   rv = apr_thread_create(&spi_thd, NULL, spi_thread, serv_ctx, context);
   assert(rv == APR_SUCCESS);

   for(;;) {
      int num, i;
      if (apr_pollset_poll(pollset, DEF_POLL_TIMEOUT, &num, &ret_pfd) == 
	    APR_SUCCESS) {
	 /* scan the active sockets */
	 for (i = 0; i < num; i++) {
	    struct callback_for_socket *callback_data = 
	       (struct callback_for_socket *)ret_pfd[i].client_data;
	    (*callback_data->callback)(callback_data->client_data);
	 }
      }
   }

   apr_atomic_set32(&serv_ctx->spi_running, FALSE);
   rv = apr_thread_join(&rv, spi_thd);
   assert(rv == APR_SUCCESS);

   apr_pool_destroy(context);
   apr_terminate();

   return 0;
}

